home *** CD-ROM | disk | FTP | other *** search
/ C/C++ Interactive Reference Guide / C-C++ Interactive Reference Guide.iso / c_ref / csource5 / 341_01 / c2ke.c < prev    next >
Text File  |  1991-02-27  |  7KB  |  215 lines
  1. /*
  2. HEADER:       c2ke.c        
  3. TITLE:        Cartesian-to-Keplerian conversion routine         
  4. VERSION:      1.0    
  5. DESCRIPTION:  This routine converts Cartesian xyz position and velocity
  6.               elements to the six classical Keplerian orbital
  7.               elements.  In addition, this routine outputs the orbit
  8.               period, true and eccentric anomalies, and semi-latus
  9.               rectum.
  10.               Required inputs are the Cartesian position and velocity
  11.               plus mu for the central body.      
  12. KEYWORDS:     Astrodynamics, orbital mechanics, Keplerian elements,
  13.               Cartesian coordinates    
  14. SYSTEM:       MS-DOS, PC-DOS (Coded with Ver. 3.3, but should be version
  15.               independent)         
  16. FILENAME:     c2ke.c    
  17. UNITS:        I believe that the code is not dependent on any particular
  18.               set of units.  See orbcons.h for the value of earth mu
  19.               I use.
  20. WARNINGS:      This routine was coded for educational purposes, using
  21.               conversion techniques given in the reference.  No attempt
  22.               has been made to provide conversions with optimal numerical
  23.               stability, although some simple checks have been included
  24.               to flag exceptional conditions, such as equatorial or circular
  25.               orbits.  Note that not every Keplerian element is defined
  26.               for every type of orbit.  For example, circular orbits have
  27.               no defined argument of periapsis (w); equatorial orbits have
  28.               no defined longitude of the ascending node (lan).
  29. SEE-ALSO:     k2ce.c, orbcons.h    
  30. AUTHORS:      Rodney Long
  31.               19003 Swan Drive
  32.               Gaithersburg, MD 20879     
  33. COMPILERS:    Microsoft C 5.1    
  34. REFERENCE:    Bate, Mueller, White: Fundamentals of Astrodynamics
  35. */
  36.  
  37. #include <stdio.h>
  38. #include <float.h>
  39. #include <math.h>
  40. #include "orbcons.h"
  41.  
  42. main()
  43. {
  44.   double c[6],k[6],mu,aux[4];
  45.   char filename[31];
  46.   FILE *fp;
  47.  
  48.   printf("Enter input file name: \n");
  49.   gets(filename);
  50.  
  51.  
  52.   fp = fopen(filename,"r");
  53.   printf("filename = %s\n",filename);
  54.   if ( fp == NULL) {
  55.     perror("Open error \n");
  56.     exit(0);
  57.   }
  58.   fscanf(fp,"%lf %lf %lf",&c[0],&c[1],&c[2]);
  59.   fscanf(fp,"%lf %lf %lf",&c[3],&c[4],&c[5]);
  60.   fscanf(fp,"%lf",&mu); 
  61.   printf("Input pos/vel: \n");
  62.   printf("%25.16lf %25.16lf %25.16lf\n",c[0],c[1],c[2]); 
  63.   printf("%25.16lf %25.16lf %25.16lf\n",c[3],c[4],c[5]);
  64.   printf("Input mu: %25.16lf\n",mu);
  65.  
  66.   c2ke(c,mu,k,aux);
  67.  
  68.   printf("Output elements (a,e,i,lan,w,m):\n");
  69.   printf("%25.16lf %25.16lf %25.16lf\n",k[0],k[1],k[2]); 
  70.   printf("%25.16lf %25.16lf %25.16lf\n",k[3],k[4],k[5]);
  71.   printf("True anom, eccen anom, semi-latus rectum: \n");
  72.   printf("%25.16lf %25.16lf %25.16lf\n",aux[0],aux[1],aux[2]);
  73.   printf("Period: \n");
  74.   printf("%25.16lf\n",aux[3]);
  75.  
  76.  
  77.   
  78. }
  79.  
  80. //  This routine converts Cartesian position and velocity
  81. //    to classical Keplerian elements, for elliptical
  82. //    orbits.
  83.  
  84. //  Input : c[6] -- pos/vel; 
  85. //          mu   -- mu of central body
  86. //  Output: k[6] -- a,e,i,lan,w,m
  87. //          ** i,lan,w,m are output in degrees **        
  88.  
  89.  
  90.   c2ke(double *c,double mu,double *k, double *aux)
  91. {
  92.         double a,e,i,lan,m,p,w;
  93.       double hmagsq, rmag, vmag, vmagsq, rdv;
  94.       double h1,h2,h3;
  95.       double x,y,z,xd,yd,zd;
  96.       double ee, e1, e2, e3;
  97.       double n1, n2;
  98.       double t, u;
  99.       double cosnu, nde, nu, period;
  100.  
  101.       x  = c[0];           // Get input pos
  102.       y  = c[1];
  103.       z  = c[2];
  104.       xd = c[3];        // Get input vel
  105.       yd = c[4];
  106.       zd = c[5];
  107.  
  108.       rmag   = sqrt(x*x   + y*y   + z*z);   // Get pos/vel mag
  109.       vmagsq = xd*xd + yd*yd + zd*zd;
  110.       vmag   = sqrt(vmagsq);
  111.       rdv    = x*xd + y*yd +z*zd;                // Get pos dot vel
  112.  
  113.  
  114.  
  115.       // Calculate angular momentum vector hbar =
  116.       //   (h1,h2,h3) = pos cross vel.
  117.       //   hbar is orthogonal to the orbit plane.    
  118.       h1 = y*zd - z*yd; 
  119.       h2 = z*xd - x*zd;
  120.       h3 = x*yd - y*xd;
  121.         hmagsq = h1*h1 + h2*h2 + h3*h3;
  122.  
  123.  
  124.         // Node vector nbar =
  125.       //   (-h2,h1,0).
  126.       //   nbar is in the orbit plane and points from
  127.       //   orbit focus to the ascending node.
  128.       n1 = -h2;
  129.       n2 =  h1;
  130.  
  131.          // Calculate eccentricity vector ebar =
  132.       //  (e1,e2,e3).
  133.       //  ebar is in the orbit plane and points from
  134.       //  the orbit focus toward periapis.  The magnitude
  135.       //  of ebar is the eccentricity.
  136.       t = vmagsq - mu/rmag;
  137.       u = 1/mu;
  138.       e1    = u * ( t * x - rdv * xd);
  139.       e2 = u * ( t * y - rdv * yd);
  140.       e3 = u * ( t * z - rdv * zd);
  141.  
  142.  
  143.                             // Calculate semi-latus rectum
  144.       p   = hmagsq/mu;
  145.                                 // Calculate eccentricity
  146.       e = sqrt(e1*e1 + e2*e2 + e3*e3);
  147.                          // Calculate semi-major axis
  148.       if ( e != 1 ) {
  149.         a = p/(1 - e*e);     
  150.       } else {
  151.         a = 0;
  152.         printf("a is infinite; orbit is parabolic \n");
  153.       }
  154.                          // Calculate inclination
  155.       i    = atan2(sqrt(h1*h1 + h2*h2),h3);
  156.       if (i < 0) i += TWOPI;
  157.                          // Calculate long of asc node
  158.       if (i == 0 ) {
  159.         lan = 0;
  160.         printf("lan is undefined; orbit is equatorial. \n");
  161.       } else {
  162.         // lan is angle between node vector nbar and
  163.         //   the positive x-axis.
  164.         lan = atan2(h1,-h2);
  165.         if (lan < 0 ) lan += TWOPI;
  166.       }
  167.  
  168.                         // Calculate arg of periapsis
  169.       if ( e != 0 && ( fabs(n1) + fabs(n2) != 0 ) ) {
  170.         nde = n1 * e1 + n2 * e2;
  171.         w = acos(nde/(sqrt(n1*n1 + n2*n2) * e));
  172.         if ( e3 < 0 ) {
  173.           w = TWOPI - w;
  174.         }
  175.       } else {
  176.         printf("e or node vec is zero; arg of perigee is undefined \n");
  177.         w = 0;
  178.       }
  179.        
  180.  
  181.                         //    Calculate true anomaly
  182.       cosnu = (e1*x + e2*y + e3*z)/(e * rmag);
  183.       nu    = acos(cosnu);
  184.       if (rdv <= 0 ) {
  185.         nu = TWOPI - nu;
  186.       }
  187.       
  188.                             // Calculate eccentric anomaly
  189.       ee = acos((e + cosnu)/(1+ e* cosnu));
  190.       if (rdv < 0) {
  191.         ee = TWOPI - ee;
  192.       } 
  193.                         // Calculate mean anomaly
  194.       m = ee - e*sin(ee);
  195.  
  196.                         // Calculate period
  197.       period = (TWOPI/sqrt(mu)) * pow(a,1.5);
  198.  
  199.  
  200.       // Output results
  201.  
  202.       k[0] = a;          // Semi-major axis             
  203.       k[1] = e;          // Eccentricity
  204.       k[2] = i  *RTD;    // Inclination
  205.       k[3] = lan*RTD;    // Long of ascending node
  206.       k[4] = w  *RTD;    // Arg of periapsis
  207.       k[5] = m  *RTD;    // Mean anomaly
  208.  
  209.       aux[0] = nu *RTD;  // True anomaly
  210.       aux[1] = ee *RTD;  // Eccentric anomaly
  211.       aux[2] = p;        // Semi-latus rectum
  212.       aux[3] = period;   // Period 
  213.       return(0);
  214. }
  215.